#pragma once
#ifndef __TOOLS_H
#define __TOOLS_H
#include <fstream>
#include <opencv2/opencv.hpp>
#include <ceres/ceres.h>
#include <Eigen/Dense>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
// #include "visualization_msgs/msg/marker.hpp"
// #include "visualization_msgs/msg/marker_array.hpp"
#include "interfaces/msg/pose.hpp"
#include "interfaces/msg/target.hpp"


#include "../../../tool/include/types.h"
// #include "../aimbot/aimbot.h"
// #include "../armor/armor.h"
// #include "geometry_msgs/msg/pose_stamped.hpp"

// #include "../autoaim_node.hpp"

extern bool P3P_HERO(const std::vector<cv::Point3f>& objectPoints, const std::vector<cv::Point2f>& imagePoints, const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs, cv::Mat& rvec, cv::Mat& tvec);
// extern bool P3P_HERO(const std::vector<cv::Point3f>& objectPoints, std::vector<cv::Point2f>& imagePoints, const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs, cv::Mat& rvec, cv::Mat& tvec);

extern void camera_to_pixel(const cv::Mat& X_camera, cv::Mat& X_pixel, const cv::Mat& cametaMatrix);//反投影函数

extern void armor_to_camera(const cv::Mat& X_armor, cv::Mat& X_camera, const cv::Mat& rvec, const cv::Mat& tvec);

extern void camera_to_armor(const cv::Mat& X_camera, cv::Mat& X_armor, const cv::Mat& rvec, const cv::Mat& tvec);

extern void camera_to_ptz(const cv::Mat& X_camera, cv::Mat& X_ptz, const cv::Mat& offset);

extern void ptz_to_camera(const cv::Mat& X_ptz, cv::Mat& X_camera, const cv::Mat& offset);

extern void ptz_to_land(const cv::Mat& X_ptz, cv::Mat& X_land, const Pose& pose);

extern void land_to_ptz(const cv::Mat& X_land, cv::Mat& X_ptz, const Pose& pose);

extern double estimate_o_yaw(const cv::Mat& tvec, const cv::Mat& offset, const Pose& pose, const double& half_w, const double& half_h, const double& armor_pitch, const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs, const cv::Mat* originalPoints);

template <typename T>
class circularQueue
{
public:
    T* arr = nullptr;
    int length = 0;
    int head = 0;
    int tail = 0;

    circularQueue(const int& length)
    {
        delete[] arr;
        arr = new T[length + 1];
        this->length = length;
        head = 0;
        tail = 0;
    }
    ~circularQueue(void)
    {
        delete[] arr;
    }
    circularQueue(void)
    {
        arr = nullptr;
    }

    bool init(const int& length)
    {
        delete[] arr;
        arr = new T[length + 1];
        this->length = length;
        head = 0;
        tail = 0;

        return 1;
    }

    bool clear(void)
    {
        head = 0;
        tail = 0;
        return 1;
    }

    bool is_full(void)
    {
        return head == (tail + 1) % (length + 1);
    }

    bool is_empty(void)
    {
        return head == tail;
    }

    int num_of_elem(void)
    {
        return (tail - head + length + 1) % (length + 1);
    }

    void push(const T& value)
    {
        if (is_full())//如果队列已满
        {
            head = (head + 1) % (length + 1);//抛弃队首值
        }
        arr[tail] = value;
        tail = (tail + 1) % (length + 1);
    }

    void pop(T& value)
    {
        if (is_empty())
        {
            std::cerr << "Error! Queue is empty!" << std::endl;
            return;
        }
        value = arr[head];
        head = (head + 1) % (length + 1);
    }

    T& operator[](const int& index)//自然是从队首（最旧值）开始，如index=0表示队首值
    {
        int index_tmp = index;
        while (1)
        {
            if (index_tmp > num_of_elem() - 1)
            {
                index_tmp = index_tmp - num_of_elem();
            }
            else if (index_tmp < 0)
            {
                index_tmp = index_tmp + num_of_elem();
            }
            else
            {
                break;
            }
        }
        return arr[(head + index_tmp) % (length + 1)];
    }
};


template <typename T>
class autoaim_filter
{
public:
    circularQueue<T> Queue;
    int length = 0;

    autoaim_filter(const int& length)
    {
        Queue.init(length);
        this->length = length;
    }
    autoaim_filter(void)
    {
        length = 0;
    }
    ~autoaim_filter(void)
    {}

    bool init(const int& length)
    {
        Queue.init(length);
        this->length = length;

        return 1;
    }
    bool init(void)
    {
        Queue.init(length);

        return 1;
    }

    T f(const int& x)
    {
        // double alpha = 0.5;
        // return (2.0 * alpha - 1.0) * x + 1.0 - alpha;//若length=2,则为一阶滞后滤波

        // return 1.0;//递推平均滤波

        return 5.0*x + 1.0;//一种加权递推平均滤波
        // return 2.0 * x * x + 1.0;
    }

    T push_and_get_filtered_val(const T& value)
    {
        Queue.push(value);
        T sum_x_fx = 0;
        T sum_fx = 0;
        for (int i = 0; i < Queue.num_of_elem(); ++i)
        {
            sum_x_fx += Queue[i] * f(i);
            sum_fx += f(i);
        }
        T x_k = sum_x_fx / sum_fx;
        Queue[Queue.num_of_elem() - 1] = x_k;
        
        return x_k;
    }
};



class BallisitcSolver
{
using odeFuncwithPartialdiff = std::function<void(const std::vector<double> &, std::vector<double> &, std::vector<double> &)>;
using odeFunc = std::function<void(const std::vector<double> &, std::vector<double> &)>;
// 常量定义
double rho0 = 1.204;                 // 空气密度(海平面)
double Cd = 0.5;                     // drag coefficient
double R = 16.8e-3 / 2.0;            // 半径
double A = M_PI * R * R;             // 横截面积
double m = 3.2e-3;                   // 质量
double lambda = Cd * A * rho0 * 0.5; // 空气阻力系数
double D_m = lambda / m;             // 空气阻力系数与质量的比值
double h = 0.001;                    // 步长
double h2 = h / 2.0;                 // 步长的二分之一，提前算好减少计算量。
double h3 = h / 3.0;                 // 步长的三分之一，提前算好减少计算量。
double h6 = h / 6.0;                 // 步长的六分之一，提前算好减少计算量。
int maxIter = 1e4;                   // 最大迭代次数
double tol = 1e-2;                   // 收敛阈值
const int n = 4;                     // 状态变量的维度
double g = 9.7988;
    double v0_ = 27;
    double target_x_ = -1.0;
    double target_y_ = -1.0;
    double flight_time = 0.0;

public:
    ceres::Solver::Options options;

    BallisitcSolver()
    {
        options.linear_solver_type = ceres::DENSE_QR;
        options.initial_trust_region_radius = 5e4;
        options.function_tolerance = 0.5;
        options.logging_type = ceres::SILENT;
        // options.minimizer_progress_to_stdout = true;
    }

    ~BallisitcSolver()
    {
    }

    void set_resistance_coefficient(double _R, double _m)
    {
        R = _R;
        m = _m;
        A = M_PI * R * R;
        lambda = Cd * A * rho0 * 0.5;
        D_m = lambda / m;
    }
    void get_result(interfaces::msg::Target &target, double v0, interfaces::msg::Pose &pose)
    {
        this->target_x_ = sqrt(target.x * target.x + target.y * target.y);
        this->target_y_ = target.z;
        this->v0_ = v0;
        pose.ptz_pitch = this->solveGunElevation();
        pose.ptz_yaw = atan2(target.y, target.x);
        pose.visual_time = target.clock + this->flight_time;
        // std::cout << flight_time << std::endl;
    }
    void intrinsic_to_extrinsic(interfaces::msg::Pose &src, interfaces::msg::Pose &dst)
    {
        // 默认传入的都是弧度，输出也是弧度

        /* 当前内旋的顺序是z-y'-x'' 需要转换的外旋顺序也是 z-y-x
        z-y'-x''内旋方式旋转矩阵的构成
        R = R(yaw)*R(pitch)*R(roll) = [cos(yaw)*cos(pitch), cos(yaw)*sin(pitch)*sin(roll) - sin(yaw)*cos(roll), cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll)],
                                      [sin(yaw)*cos(pitch), sin(yaw)*sin(pitch)*sin(roll) + cos(yaw)*cos(roll), sin(yaw)*sin(pitch)*cos(roll) - cos(yaw)*sin(roll)],
                                      [-sin(pitch),         cos(pitch)*sin(roll),                               cos(pitch)*cos(roll)]
        z-y-x外旋
        R = R(roll)*R(pitch)*R(yaw) = [cos(pitch)*cos(yaw)                               ,-cos(pitch)*sin(yaw)                              ,sin(pitch)],
                                      [cos(roll)*sin(yaw) + sin(roll)*sin(pitch)*cos(yaw),cos(roll)*cos(yaw) - sin(roll)*sin(pitch)*sin(yaw),-sin(roll)*cos(pitch)],
                                      [sin(roll)*sin(yaw) - cos(roll)*sin(pitch)*cos(yaw),sin(roll)*cos(yaw) + cos(roll)*sin(pitch)*sin(yaw),cos(roll)*cos(pitch)]

        */

        Eigen::Matrix3d rotate_mat;
        rotate_mat = Eigen::AngleAxisd(src.ptz_yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(-src.ptz_pitch, Eigen::Vector3d::UnitY());
        // 这是内旋的

        // 根据外旋的矩阵来计算外旋时yaw和pitch
        dst.ptz_pitch = -asin(rotate_mat(0, 2));
        dst.ptz_yaw = -atan2(rotate_mat(0, 1), rotate_mat(0, 0));
        dst.ptz_roll = src.ptz_roll;

        dst.visual_time = src.visual_time;
    }

private:
    double calculateInitialTheta()
    {
        double dis = std::sqrt(target_x_ * target_x_ + target_y_ * target_y_);
        double phi = std::atan2(target_y_, target_x_);
        double alpha = std::asin((target_y_ + g * target_x_ * target_x_ / (v0_ * v0_)) / dis);
        double theta1 = (phi + alpha) / 2;
        double theta2 = M_PI - (phi + alpha) / 2;
        double theta = (theta1 + M_PI / 2 > theta2 + M_PI / 2) ? theta2 : theta1;
        if (fabs(theta) > M_PI / 2)
        {
            RCLCPP_ERROR_STREAM(rclcpp::get_logger("rune_trajectory"), "error,t1:" << theta1 << "t2:" << theta2 << target_x_ << "," << target_y_ << std::endl);
        }
        this->flight_time = dis/this->v0_/std::cos(theta);

        return theta;
    }

    double
    solveGunElevation()
    {
        double theta0 = calculateInitialTheta();

        std::cout << "theta0:" << theta0 * 180 / M_PI;
        ceres::Problem problem;
        ceres::CostFunction *cost_function =
            new ceres::NumericDiffCostFunction<BallisticCostFunctor, ceres::FORWARD, 2, 1>(
                new BallisticCostFunctor(this));
        problem.AddResidualBlock(cost_function, nullptr, &theta0);

        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        // std::cout << "total:" << summary.total_time_in_seconds << "theta:" << theta0 * 180 / M_PI << "flight_time:" << this->flight_time << std::endl;
        // RCLCPP_INFO(rclcpp::get_logger("auto_aim"),"time:%lf",summary.total_time_in_seconds);

        return theta0;
    }

    struct BallisticCostFunctor
    {
        BallisticCostFunctor(BallisitcSolver *solver) : solver_(solver) {}

        bool operator()(const double *const theta0, double *residuals) const
        {
            const double v0 = solver_->v0_;
            const double target_x = solver_->target_x_;
            const double target_y = solver_->target_y_;

            std::vector<double> y0 = {0, v0 * cos(*theta0), 0, v0 * sin(*theta0)};
            std::vector<double> yTarget = {target_x, 0, target_y, 0};
            std::vector<double> y(4), ddydt_dtheta(4);
            y = solver_->odesolve(y0, yTarget, ddydt_dtheta);

            residuals[0] = (y[2] - target_y) * ddydt_dtheta[2];
            residuals[1] = (y[0] - target_x) * ddydt_dtheta[0];

            return true;
        }

        BallisitcSolver *solver_;
    };

    std::vector<double> odesolve(const std::vector<double> &y0, const std::vector<double> &yTarget,
                                 std::vector<double> &ddydt_dtheta)
    {
        auto f = [this](const std::vector<double> &y, std::vector<double> &dydt, std::vector<double> &ddydt_dtheta)
        {
            double v = sqrt(y[1] * y[1] + y[3] * y[3]);
            double D = D_m * v * v; // 这里直接算得是阻力带来的加速度，因为质量可以提前算入比值中，减少计算量。
            double cos_theta = y[1] / v;
            double sin_theta = y[3] / v;

            dydt[0] = y[1];
            dydt[1] = -D * cos_theta;
            dydt[2] = y[3];
            dydt[3] = -D * sin_theta - g;

            ddydt_dtheta[0] = -v * sin_theta;
            ddydt_dtheta[1] = D * sin_theta;
            ddydt_dtheta[2] = v * cos_theta;
            ddydt_dtheta[3] = -D * cos_theta;
        };

        std::vector<double> y = y0;
        std::array<std::vector<double>, 8> ks;
        for (auto &k : ks)
        {
            k.resize(n);
        }
        std::vector<double> ytemp(n);
        int iter = 0;
        this->flight_time = 0;
        bool converged = false;

        while (!converged && iter < maxIter)
        {
            // rungeKutta4(f, this->flight_time, y, h, dydt, ddydt_dtheta);
            rungeKutta4(f, this->flight_time, y, ddydt_dtheta, ks, ytemp);
            iter++;

            if (y[0] > yTarget[0])
            {
                converged = true;
            }
            double error = 0.0;
            for (int i = 0; i < n; i++)
            {
                error += fabs(y[i] - yTarget[i]);
            }
            if (error < tol)
            {
                converged = true;
            }
        }
        // std::cout << "Final state: ";
        // for (double yi : y)
        // {
        //     std::cout << yi << " ";
        // }
        // std::cout << std::endl;

        return y;
    }

    // Runge-Kutta四阶法实现
    void rungeKutta4(odeFuncwithPartialdiff f, double &t, std::vector<double> &y, std::vector<double> &ddydt_dtheta, std::array<std::vector<double>, 8> &ks, std::vector<double> &ytemp)
    {
        f(y, ks[0], ks[4]); // k1, k1_theta

        for (int i = 0; i < n; ++i)
        {
            ytemp[i] = y[i] + h2 * ks[0][i];
        }

        f(ytemp, ks[1], ks[5]); // k2, k2_theta

        for (int i = 0; i < n; ++i)
        {
            ytemp[i] = y[i] + h2 * ks[1][i];
        }

        f(ytemp, ks[2], ks[6]); // k3, k3_theta

        for (int i = 0; i < n; ++i)
        {
            ytemp[i] = y[i] + h * ks[2][i];
        }

        f(ytemp, ks[3], ks[7]); // k4, k4_theta

        for (int i = 0; i < n; ++i)
        {
            y[i] += h6 * ks[0][i] + h3 * (ks[1][i] + ks[2][i]) + h6 * ks[3][i];
            ddydt_dtheta[i] = h6 * ks[4][i] + h3 * (ks[5][i] + ks[6][i]) + h6 * ks[7][i];
        }
        t += h;
    }

    // Runge-Kutta四阶法实现
    void rungeKutta4(odeFunc f, double &t, std::vector<double> &y, std::vector<double> &ks, std::vector<double> &ytemp)
    {
        f(y, ks); // k1

        for (int i = 0; i < n; ++i)
        {
            ytemp[i] = y[i] + h2 * ks[i];
            y[i] += h6 * ks[i];
        }

        f(ytemp, ks); // k2

        for (int i = 0; i < n; ++i)
        {
            ytemp[i] = y[i] + h2 * ks[i];
            y[i] += h3 * ks[i];
        }

        f(ytemp, ks); // k3

        for (int i = 0; i < n; ++i)
        {
            ytemp[i] = y[i] + h * ks[i];
            y[i] += h3 * ks[i];
        }

        f(ytemp, ks); // k4

        for (int i = 0; i < n; ++i)
        {
            y[i] += h6 * ks[i];
        }
        t += h;
    }
};



#endif